#include "frustum.h"	// Engine
#include "util/mmgr.h"		// Needs to come in early

using namespace math;

Frustum::Frustum()
{
	for (int i=0; i<PlaneCount; i++)
	{
		m_PlaneA[i].m_Normal = Vec3(0.0f, 0.0f, 0.0f);
		m_PlaneA[i].m_D;
	}
}

Frustum::Frustum(const Frustum& _Frust)
{
    Clone(_Frust);
}


Frustum::~Frustum()
{

}


void    Frustum::Clone(const Frustum& _Frust)
{
    for (unsigned int i=0; i<PlaneCount; i++)
    {
        m_PlaneA[i].m_D         = _Frust.m_PlaneA[i].m_D;
        m_PlaneA[i].m_Normal    = _Frust.m_PlaneA[i].m_Normal;
    }
}

bool	Frustum::CalculateFrustrum(const Mat4x4& _mMat, const Mat4x4& _mProj, bool _bNormalize, bool _bInvert)
{
	Mat4x4 mCamera;
//	FIXME:  Re-enable
//	D3DXMatrixMultiply(&mCamera, &_mMat, &_mProj);

	// Left clipping plane
	m_PlaneA[LeftPlane].m_Normal	= Vec3(	mCamera._14 + mCamera._11,
									mCamera._24 + mCamera._21,
									mCamera._34 + mCamera._31);
	m_PlaneA[LeftPlane].m_D	= mCamera._44 + mCamera._41;

	
	// Right clipping plane
	m_PlaneA[RightPlane].m_Normal	= Vec3(	mCamera._14 - mCamera._11,
									mCamera._24 - mCamera._21,
									mCamera._34 - mCamera._31);
	m_PlaneA[RightPlane].m_D	= mCamera._44 - mCamera._41;

	// Top clipping plane
	m_PlaneA[TopPlane].m_Normal	= Vec3(	mCamera._14 - mCamera._12,
									mCamera._24 - mCamera._22,
									mCamera._34 - mCamera._32);
	m_PlaneA[TopPlane].m_D	= mCamera._44 - mCamera._42;

	// Bottom clipping plane
	m_PlaneA[BottomPlane].m_Normal	= Vec3(	mCamera._14 + mCamera._12,
									mCamera._24 + mCamera._22,
									mCamera._34 + mCamera._32);
	m_PlaneA[BottomPlane].m_D	= mCamera._44 + mCamera._42;

	// Near clipping plane
	m_PlaneA[NearPlane].m_Normal	= Vec3(	mCamera._13,
									mCamera._23,
									mCamera._33);
	m_PlaneA[NearPlane].m_D	= mCamera._43;

	
	// Far clipping plane
	m_PlaneA[FarPlane].m_Normal	= Vec3(	mCamera._14 - mCamera._13,
									mCamera._24 - mCamera._23,
									mCamera._34 - mCamera._33);
	m_PlaneA[FarPlane].m_D	= mCamera._44 - mCamera._43;


	// Normalize the plane equations, if requested
	if (_bNormalize )
	{
		float fLength;
		float fInvLength;
		for (int i=0; i<PlaneCount; i++)
		{
			fLength = m_PlaneA[i].m_Normal.Magnitude();
			fInvLength = 1.0f / fLength;
			m_PlaneA[i].m_Normal = m_PlaneA[i].m_Normal * fInvLength;
			m_PlaneA[i].m_D *= fInvLength;
		}
	}

	if (_bInvert)
	{
		for (int i=0; i<PlaneCount; i++)
		{
			m_PlaneA[i].m_Normal = -m_PlaneA[i].m_Normal;
			m_PlaneA[i].m_D = -m_PlaneA[i].m_D;
		}
	}

	return true;	
}

math::Frustum::FrustumTest Frustum::TestAABB(const Vec3& _vMin, const Vec3& _vMax) const 
{
	Vec3				vMin, vMax; 
	bool				bIntersect	= false;

	for (int i = 0; i < PlaneCount; i++) 
	{ 
		SwapIfNeg(m_PlaneA[i].m_Normal.x, _vMin.x, _vMax.x, vMin.x, vMax.x);
		SwapIfNeg(m_PlaneA[i].m_Normal.y, _vMin.y, _vMax.y, vMin.y, vMax.y);
		SwapIfNeg(m_PlaneA[i].m_Normal.z, _vMin.z, _vMax.z, vMin.z, vMax.z);

		if (DistanceToPoint(&m_PlaneA[i], &vMin) > 0.0f)
		{
			return FrustumOutside;	// FrustumOutside the point
		}

		if (DistanceToPoint(&m_PlaneA[i], &vMax) >= 0.0f)
		{
			bIntersect = true; 
		}
	} 

	if (bIntersect) 
	{
		return FrustumPartial; 
	}
	return FrustumInside; 
}


math::Frustum::FrustumTest Frustum::TestPoint(const Vec3& _vPos) const 
{
	////////////////////////////////////////////////////////////////////////
	// Return true when the given point is inside the view frustum. This is
	// the case when its distance is positive to all the frustum planes
	////////////////////////////////////////////////////////////////////////

	for (int i = 0; i < PlaneCount; i++) 
	{ 
		if (DistanceToPoint(&m_PlaneA[i], &_vPos) >= 0.0f)
		{
			return FrustumOutside;	// Points are either or...
		}
	}

   return FrustumInside;
}



math::Frustum::FrustumTest Frustum::TestSphere(const Vec3& _vPos, float _fRad) const 
{
	////////////////////////////////////////////////////////////////////////
	// Return true when the given point is inside the view frustum. This is
	// the case when its distance is positive to all the frustum planes
	////////////////////////////////////////////////////////////////////////

	unsigned short usInside = 0;
	for (int i = 0; i < PlaneCount; i++) 
	{ 
		float m_D = -DistanceToPoint(&m_PlaneA[i], &_vPos);

		if ( m_D <= -_fRad)
		{
			return FrustumOutside;	// Sphere is outside
		}
		if ( m_D > _fRad)
		{
			usInside++;
		}
	}

	if (usInside == 5) return FrustumInside;
   return FrustumPartial;
}

math::Frustum::FrustumTest Frustum::TestOBB(Vec3 _vPoint[]) const 
{
	return FrustumOutside;	// Test not created yet.
}





/////////
// Protected functions

void Frustum::SwapIfNeg(float _fSign, float _fFrom1, float _fFrom2, float& _fTo1, float&_fTo2) const 
{
	if (_fSign >= 0.0f)
	{
		_fTo1 = _fFrom1;
		_fTo2 = _fFrom2;
	} else
	{
		_fTo1 = _fFrom2;
		_fTo2 = _fFrom1;
	}
}

float Frustum::DistanceToPoint(const Plane* _pPlane, const Vec3* _pPoint) const
{
    float fResult = (_pPlane->m_Normal.x * _pPoint->x) + 
					(_pPlane->m_Normal.y * _pPoint->y) + 
					(_pPlane->m_Normal.z * _pPoint->z) + 
					_pPlane->m_D;
	return fResult;
}